//
// Created by oftenlin on 23-9-13.
//

#ifndef LANGGE_VDR_ESKFFUSIONSIMPLE_HPP
#define LANGGE_VDR_ESKFFUSIONSIMPLE_HPP
#include "NaviFusionImpl.hpp"
class ESKFFusionSimple :INaviFusion{
public:
    // 初始化
    void Init(NaviData::NavInfo &initPos,uint32_t state_num,uint32_t noise_num) override;
    // 预测
    void Predict(std::shared_ptr<NaviData::ImuData>& imuDataPtr) override;
    // 修正
    void Correct(std::shared_ptr<NaviData::BaseData>& naviDataPtr) override;
    // 对外的位置姿态接口
    void GetPos(NaviData::NavInfo &pos) override;
    // 雅可比矩阵更新
    void UpdateJacobianMatrix() override;
    bool initialized() const{
        return initialized_;
    }
private:

    NaviData::ImuData::ImuDataDetail currImu;
    NaviData::ImuData::ImuDataDetail preImu;
    bool ComputeDeltaAngular(Eigen::Vector3d& delta_angular);
    // 去除偏置 的 加速度
    Eigen::Vector3d f_;
    // 重力加速度
    Eigen::Vector3d grav_{0,0,9.807};
    Eigen::MatrixXd baseQ;

    void INSSimple();
    void INSDetail();



    void UpdatePredictNoise(double dt);

    void CorrectByGNSSData(std::shared_ptr<NaviData::GnssData>& gnssDataPtr);

    void CorrectByCANSpeedData(std::shared_ptr<NaviData::CANVelocity>& canVelocityPtr);

    void FeedBack();

    void imuCompensate(NaviData::NavInfo &pvapre, NaviData::ImuData::ImuDataDetail &imucur);
    void velUpdate(const NaviData::NavInfo &pvapre, NaviData::NavInfo &pvacur, const NaviData::ImuData::ImuDataDetail &imupre,
              const NaviData::ImuData::ImuDataDetail &imucur);
    void posUpdate(const NaviData::NavInfo &pvapre, NaviData::NavInfo &pvacur, const NaviData::ImuData::ImuDataDetail &imupre,
              const NaviData::ImuData::ImuDataDetail &imucur);
    void attUpdate(const NaviData::NavInfo &pvapre, NaviData::NavInfo &pvacur, const NaviData::ImuData::ImuDataDetail &imupre,
                   const NaviData::ImuData::ImuDataDetail &imucur);
    Eigen::Vector3d acce_bias_;
    Eigen::Vector3d gyro_bias_;
};


#endif //LANGGE_VDR_ESKFFUSIONSIMPLE_HPP
